from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os
from launch.substitutions import PathJoinSubstitution,PythonExpression

def generate_launch_description():
    pkg_sls_launch_sim = get_package_share_directory('sls_launch_sim')
    return LaunchDescription([
        # 声明可配置参数
        DeclareLaunchArgument(
            'sdf_file',
            default_value='x500_under_range',
            description='Path to the SDF model file'
        ),
        DeclareLaunchArgument(
            'model_name',
            default_value='my_model',
            description='Name of the model to spawn'
        ),
        DeclareLaunchArgument(
            'x',
            default_value='0.0',
            description='X position of the model'
        ),
        DeclareLaunchArgument(
            'y',
            default_value='0.0',
            description='Y position of the model'
        ),
        DeclareLaunchArgument(
            'z',
            default_value='0.0',
            description='Z position of the model'
        ),
        DeclareLaunchArgument(
            'roll',
            default_value='0.0',
            description='Roll orientation of the model (radians)'
        ),
        DeclareLaunchArgument(
            'pitch',
            default_value='0.0',
            description='Pitch orientation of the model (radians)'
        ),
        DeclareLaunchArgument(
            'yaw',
            default_value='0.0',
            description='Yaw orientation of the model (radians)'
        ),
        
        
        # 等待Gazebo启动完成
        ExecuteProcess(
            cmd=['sleep', '5'],
            output='screen'
        ),
        
        # 使用参数化值生成服务调用命令
        ExecuteProcess(
            cmd=[
                'gz', 'service', '-s', '/world/default/create',  # 服务名称单独作为一个参数
                '--reqtype', 'gz.msgs.EntityFactory',
                '--reptype', 'gz.msgs.Boolean',
                '--timeout', '1000',
                '--req',
                [
                    'sdf_filename: "',
                    PathJoinSubstitution([pkg_sls_launch_sim, 'models', LaunchConfiguration('sdf_file'), 'model.sdf']),
                    '", name: "', LaunchConfiguration('model_name'), '", ',
                    'pose: { position: { x: ', LaunchConfiguration('x'), 
                    ', y: ', LaunchConfiguration('y'), 
                    ', z: ', LaunchConfiguration('z'), ' }, ',
                    'orientation: { w: ',
                    PythonExpression(['cos(', LaunchConfiguration('roll'), '/2)*cos(', LaunchConfiguration('pitch'), '/2)*cos(', LaunchConfiguration('yaw'), '/2) + sin(', LaunchConfiguration('roll'), '/2)*sin(', LaunchConfiguration('pitch'), '/2)*sin(', LaunchConfiguration('yaw'), '/2)']),
                    ', x: ',
                    PythonExpression([
                        'sin(', LaunchConfiguration('roll'), '/2)*cos(', LaunchConfiguration('pitch'), '/2)*cos(', 
                        LaunchConfiguration('yaw'), '/2) - cos(', LaunchConfiguration('roll'), '/2)*sin(', 
                        LaunchConfiguration('pitch'), '/2)*sin(', LaunchConfiguration('yaw'), '/2)'
                    ]),
                    ', y: ',
                    PythonExpression([
                        'cos(', LaunchConfiguration('roll'), '/2)*sin(', LaunchConfiguration('pitch'), '/2)*cos(', 
                        LaunchConfiguration('yaw'), '/2) + sin(', LaunchConfiguration('roll'), '/2)*cos(', 
                        LaunchConfiguration('pitch'), '/2)*sin(', LaunchConfiguration('yaw'), '/2)'
                    ]),
                    ', z: ',
                    PythonExpression([
                        'cos(', LaunchConfiguration('roll'), '/2)*cos(', LaunchConfiguration('pitch'), '/2)*sin(', 
                        LaunchConfiguration('yaw'), '/2) - sin(', LaunchConfiguration('roll'), '/2)*sin(', 
                        LaunchConfiguration('pitch'), '/2)*cos(', LaunchConfiguration('yaw'), '/2)'
                    ]),
                    ' } }'
                ]
            ],
            output='screen'
        )
  
    ])